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ABSTRACT, l’his paper deals with tlie class of problems that involve finding where to place or how 
to move a solid object in the presence of obstacles. The solution to this class of problems is essential 
to the automatic planning of manipulator transfer movements, i.e. the motions to grasp a part and 
place it at some destination. This paper presents algorithms for planning manipulator paths that 
avoid collisions with objects in the workspace and for choosing safe grasp points on objects. These 
algorithms allow planning transfer movements for cartesian manipulators. The approach is based on 
a method of computing an explicit representation of the manipulator configurations that would bring 
about a collision [27]. 
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1. Introduction 

An important goal of research on programming languages for computer-controlled manipulators 
is a language in which assembly operations can be concisely described. Two major approaches to 
manipulator programming have been identified [34]: 

1. Explicit programming — in which the user specifics all the manipulator motions needed to 
accomplish a task. 

2. Model-based programming — in which the user specifies geometric models of parts and a 
description of die task in terms of dicse models. The detailed manipulator motions are derived 
by die assembly system from these specifications. 


This paper presents algorithms for some of the central geometric problems diat arise in die model- 
based approach to manipulator programming. In particular, this paper deals with the class of 
problems that involve finding where to place or how to move a solid object in the presence of 
obstacles. The solution to this class of problems is essential to the automatic planning of manipulator 
transfer movements, i.c. the motions to grasp a part and place it at some destination. For example, 
planning transfer movements requires the ability to plan padis for the manipulator diat avoid colli¬ 
sions with objects in die workspace and the ability to choose safe grasp points on objects. The ap¬ 
proach to dicse problems described here is based on a method of computing an explicit representation 
of the manipulator configurations that would bring about a collision [27]. 

Several model-based manipulator systems have been described in the recent literature: AL [10] 
[46], Autopass [24] [28] [50], TAMA [25] [26] and RAFT [38] [39] [40]. These are experimental systems, 
currently under development 1 . Work on die model-based aspects of AL has focused on techniques 
for making coding decisions in manipulator programs. r Hic decisions arc made among a fixed set of 
strategics so as to minimize estimated execution times and so as to bring estimates on the accuracy 

i 

of part positions within specified bounds. A central technical issue in diis approach is deriving the 
accuracy estimates from geometric relationships and local accuracy information. RAPP has focused 
on the specification of manipulator programs by specifying the desired symbolic spatial relation¬ 
ships among objects. These relations are dicn translated into algebraic constraints on the position 


'The AL language, as originally described, includes explicit as well as model-based programming capabilities. The former 
are currently available, while the latter are still in the experimental stage. 
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parameters of the objects, which can be solved by symbolic manipulation. These algebraic solution 
techniques are also used to complete the specification of partially specified actions so as to achieve 
the desired relationships. Implementation work on I. AM A and Autopass has focused on techniques 
for planning collision-free motions, e.g. grasping and parts transfer motions, using polyhedral object 
models. The techniques reported in this paper arc extensions of the Autopass obstacle avoidance 
algorithm and LAMA’s grasping strategies. 

A number of important problems relevant to model-based manipulator programming have been 
addressed independently of any manipulator system, for example the problem of specifying com¬ 
pliant motion strategies based on geometric and kinematic models of a task [30], the selection of 
grasping positions [5] [31] [35] [51], and the problem of collision detection and collision avoidance 
among obstacles [3] [7] [12] [33] [47], 

The algorithms discussed in this paper are based on previous work on obstacle avoidance algo¬ 
rithms. In particular, [48] [49] first formulated the obstacle avoidance problem in terms of an obstacle 
transformation which allows treating die moving object as a point. A similar transformation was also 
used in [1] [2] [4] [45] for the template layout problem; related applications arc also discussed in [11] 
[16]. Generalizations of these obstacle transformation techniques and a review of related work can be 
found in [27] and [28]. Other approaches to automatic obstacle avoidance arc reviewed in [23] [48], 

2. The "Pick and Place" Synthesis Problem 

The most common transfer movements arc of die "pick and place" type, consisting of (1) moving 
the manipulator from its current configuration 2 to a grasp configuration on some object, P, (2) grasp¬ 
ing P and (3) moving P to some specified configuration. The "pick and place" synthesis problem is 
that of deriving the manipulator motions that will carry out a "pick and place" transfer movement, 
given as input die following data; 

. 1. a geometric description of the manipulator and the objects in the workspace, 

2. die current configurations of the manipulator and die objects in the workspace, 

2 Configuration will be used here to refer to the combined position and orientation of an object as well as to the set 
of joint parameters specifying the arrangement of manipulator links. 
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3. the desired final configuration of P, and 

4. (Optional) the grasp configuration on P. 

Phis paper focuses on the geometric aspects of the "pick and place" synthesis problem. For 
example, when the grasp configuration is known, the "pick and place" synthesis problem is equivalent 
to finding collision-free paths for the manipulator and P between the configurations in items 2, 3 
and 4 above; when the grasp configuration is unknown, there is the additional task of choosing a 
configuration such that: 

1. the manipulator’s fingers are in contact with P, 

2. the manipulator does not collide with nearby objects, 

3. the configuration is reachable, and 

4. the object is stable in the manipulator’s hand. 

The first three conditions reflect geometric constraints on the manipulator configuration, relative to P 
and to other objects in the workspace. The stability condition reflects aspects of grasping beyond the 
purely geometric, but when P is small relative to the manipulator hand and when parts mating effects 
arc ignored, then stability considerations can typically be reduced to geometric heuristics (sec Section 
9.6). 

The geometric aspects of "pick and place" can be formulated in terms of two fundamental spatial 
planning problems [27], Findspacc and Findpath, which occur in many applications. The definition of 
these basic problems are presented below for the case of polyhedral objects. 

Let R be a convex polyhedron that bounds the workspace and which contains ko other, possibly 
overlapping, convex polyhedra B, designated as obstacles. Let A, the object being moved, be the 
union of convex polyhedra Aj, i.c. A = Ui=i M- 

1. Findspacc — Find a configuration for A, inside R, such that VzVj: Ai 0 Bj — 0. This is 

called a safe configuration. 

2. Findpath — Find a path for A from configuration s to configuration g such that A is always 

in R and A never overlaps any of the Bj. Ihis is called a safe path. 



4 The Cspace Approach lo Spatial Planning: Overview 

Clearly, "pick and place" with known grasp configuration can be viewed as a sequence of two 
Findpath problems. In addition, the configurations which are legal candidates for grasping can be 
derived from solutions to the Findspace problem. 

Fhc reduction of the "pick and place" problem to these more fundamental geometric problems 
assumes that the locations of all objects are known to high accuracy and that the path of the 
manipulator can be controlled to the same precision. In a realistic environment, there is always uncer¬ 
tainty in the positions of objects and error in the control of the manipulator. Section 10 discusses the 
effects of uncertainty. 


3. The Cspace Approach to Spatial Planning: Overview 

In this section, an overview of the Configuration Space approach to spatial planning will be 
presented; further details can be found in [27]. 

r lhc position and orientation of a rigid solid can be specified by a single 6-dimcnsional vector, 
called its configuration. The 6-dimcnsional space of configurations for a solid. A, is called its 
Configuration Space and denoted Cspace a. For example, a configuration may have one coordinate 
value for each of the x, y, z coordinates of a selected point on the object and one coordinate value for 
each of the object’s Euler angles [21]. In general, an n-dimensional configuration space can be used 
to model any system for which the position of every point on the objcct(s) can be specified with n 
parameters. An example is the configuration of an industrial robot with n joints, where n is typically 5 
or 6. In CapaceA , the set of configurations of A where A overlaps B. i.e. A OB ^ 0, will be denoted 
CO a (B), the CapaceA Obstacle due to B. Similarly, those configurations of A where A is completely 
inside B, i.e. A C B, will be denoted CIa(B), the CapaceA Interior of B. Together, these two 
Cspace a constructs embody all the information needed to solve Findspace and Findpath problems. 
Note that CIa(B ) = — COa( — B ), where —X denotes the set complement of X in R. 


3.1. Fixed Orientation of A 

In two dimensions, if the orientation of a convex polygon A is fixed, Capace A is simply die ( x , y) 
plane. This is so because the (x, y) position of some reference vertex tva is sufficient to specify the 
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Figure 1 . The On)x>a: A obstacle due to II, for fixed orientation of A 

polygon's configuration. In this case, the presence of another convex polygon fl constrains rv A to 
be outside of CO A (B), a larger convex polygon, shown as the shaded region in Figure 1. Since 
CO \[B), in this case, is a set of (x, y ) values, it is denoted CO^ J (B). Similarly, if A and B arc three- 
dimensional polyhedra in fixed orientations, then the C space obstacles arc denoted CO% JZ (B). Thus, 
the Findspace problem for polygons, with fixed orientation, can be transformed to the equivalent 
problem of placing rv A outside of CO^ J (B), but inside CI*^(R). Similarly, for multiple obstacles fly, 
a location for A is safe if and only if rv A is not inside any of the CO^(Bj), but is inside CI a £ i {R). 

If the orientation of A is fixed, then the Findpath problem for the polygon A among the fly is 
equivalent to the Findpath problem for the point rv A among the CO% J [Bj). When die CO^ y (fly) 
are polygons, the shortest 3 safe paths for rv A arc piecewise linear paths connecting the start and the 
goal via the vertices of the CO% J (B) polygons, Figure 2. Therefore, Findpath can be formulated as a 
graph search problem. The graph is formed by connecting all pairs of vertices of Cspace A obstacles 
(and the start and goal) that can "see" each other, i.e. can be connected by a straight line that does 
not intersect any of the obstacles. The shortest path from the start to the goal in this visibility graph 

3 This assumes Euclidean distance as a metric. For the optimality conditions using a rectilinear (Manhattan) metric, see 
[ 22 ]. 
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Figure 2. This figure illustrates the Findpath problem and its formulation using Cspace obstacles. Note 
that the shortest collision-free paths connect the origin and the destination via the vertices of the CspaceA 
obstacles. 


(Vgraph) is the shortest safe path for A among the Bj [28], This algorithm solves Findpath problems 
when the orientation of A is fixed. Hut, because they require moving A along obstacle boundaries, 
shortest paths are very susceptible to inaccuracies in the object models. 

The approach to Findspace and Findpath described above generalizes to problems involving 
three dimensional polyhedra with fixed orientation. The generalization requires the use of a three- 
dimensional Cspace A , representing the space of (x, y,z) positions of rv A . In this Cspace , the 
obstacles are also polyhedra, denoted CO% JZ (B). However, the Vgraph algorithm has several addi¬ 
tional drawbacks when the obstacles are three-dimensional: 

1. Shortest paths do not typically traverse the vertices of the CO^ z (Bj). 

2. There may be no paths via vertices, within the enclosing polyhedral region R , although 

other types of safe paths within R may exist. 


'these drawbacks may be alleviated by introducing additional nodes in the Vgraph which do not 
correspond to vertices [28]. An alternative strategy for finding safe paths among two- or three- 
dimensional Cspace A obstacles is discussed in Section 7. 
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3.2. Algorithms for CO jf z (B) 

The central operation in the Cspace approach to Find.space and Find path in two and three 
dimensions is computing CO x f{B) and CO x f~{B) respectively. If/1 and/? arc convex polyhedra, it is 
simple to show [27] that 

CO*f~{B) — BQ {A )o = conv(vert{B) 0 vert{{A) 0 )) 

where conv(X) is the convex hull of X [14], vert(X) is the set of vertices of the polyhedron X, 
X QY = {x — y | x G X and y GY} and (X)o means the ‘polyhedron X in its initial 
configurations, where rv\ is at the origin. This result and the existence of 0(n log n) convex hull 
algorithms for finite sets of points in 3i 3 [41], lead directly to an 0(v 2 log v) algorithm for CO^ vz [B), 
where v = \vert(A)\ -f- |i>erf(/3)|. The result also holds when A and B arc convex polygons, but more 
efficient algorithms exist for this ease. In particular, an 0(v) algorithm for COf(B) is described in 
[27]. 

3.3. Variable Orientation of A 

When A is a three-dimensional solid which is allowed to rotate, CO A {B) is a complicated curved 
object in a six-dimensional Cspace 4 . Rather than compute these objects directly, the approach taken 
here is to use a sequence of two- and three-dimensional objects to approximate the high-dimensional 
Cspace A obstacles. In particular, a six-dimensional Cspace a obstacle for a rigid solid can be ap¬ 
proximated by projections of its (x, y, z)-slices. A y-slice of an object C G 5?” is defined to be: 


{(A, ...,Pn)€C | 7y < Pj < V,} 


Where 7 , and V arc the lower and upper bounds of the slice, respectively. Then, if K is a set of 
indices between 1 and n, a /('-slice is the intersection of all the / slices for j G K. Notice that 
a K -slice of C is an object of the same dimension as C. Slices can then be projected onto those 
coordinates not in K to obtain objects of lower dimension. 

Figure 3 shows a two-dimensional example of slice projection. The objects shown shaded repre¬ 
sent the (x, y) projection of three (/-slices of CO a (B) when7l and B arc convex polygons. These slices 
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Figure 3. Slice projections of Cspaa A obstacles computed using the (c, y)-area swept out by A over a 
range of 0 values. Pitch of the shaded obstacles is the (x, y)-projection of a 0-slicc of CO A (li). Ihe figure 
also shows a polygonal approximation to the slice projection and the polygonal approximation to the swept 
volume from which it derives. 

represent configurations where A overlaps B for some orientation of A in the specified range of 0. 
In [27] is a proof that these slice projections arc equivalent to the CO Ty of the area (volume) swept 
out by A over the range of orientations of the slice. Note that approximating the swept volume as a 
polyhedron leads to a polyhedral approximation for the projected slices of the Cspace 4 obstacles, as 
shown in Figure 3. 


Slice projection has two important properties: 

1. A solution to a Findspace problem in any of the slices is a solution to the original problem, 
but since the slices are an approximation to the Cspace 4 obstacle, the converse is not neces¬ 
sarily true. 
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Figure 4. An illustration of the Findpalh algorithm using slice projection described by Fozano-Perez and 
Wesley in [28]. A number of slice projections of the Gspace obstacles arc constructed for different ranges 
of orientations of A. The problem of planning safe paths in the high-dimensional Cspace A is decomposed 
into (1) planning safe paths via CO vertices within each slice projection and (2) moving between slices, at 
configurations that arc safe in both slices. A \ represents A in its initial configuration, A 3 represents A in its 
final configuration and . 1-2 is a simple polyhedral approximation to the swept volume of A between its initial 
and final orientation. 


2. The slice projection of a Cspace A obstacle can be computed, by using the swept volume 
operation, without having to compute the high-dimensional Cspace A obstacle, see Section 5. 


When rotations of A are allowed, the slice projection operation can be used to extend the Vgraph 
algorithm described earlier to find safe (but sub-optimal) paths [28]. A number of slice projections 
of the C space A obstacles are constructed for different ranges of orientations of A. The problem of 
planning safe paths in the high-dimensional Cspace A is decomposed into: 

1 . planning safe paths via the vertices of Cspace A obstacles within each slice projection, and 


2 . moving between slices, at configurations that arc safe in both slices. 

i 






Figure 5. Models of objects are structured as trees of convex polyhedra: internal nodes represent the union 
of their sub-trees. Linked polyhedra arc used to represent manipulators: internal nodes represent joints and 
the leaves represent links. Ibe nesting of sub-trees in the models of linked polyhedra reflect the cascading 
cITccl of joint motions. 

Roth of these types of motions can be modelled as links in the Vgraph, therefore the complete algo¬ 
rithm can be formulated as a graph search problem. This approach is illustrated in Figure 4. However, 
since the obstacles are three-dimensional, the Vgraph algorithm is subject to the drawbacks described 
earlier. 

4. Findpath for Cartesian Manipulators 

This section overviews an implementation 4 of the Findpath algorithm, for cartesian manipulators 
(see definition below). Sections 5 through 8 present a more detailed description of the implementation. 
'Hie system inputs are: 

'’ The current implementation is written in LISP for the MIT LISP Machines. 









z 



Figure 6. A schematic representation of the link arrangement in two types of existing cartesian manipulators. 

1. A polyhedral model of the workspace — where each object is represented by a tree of 
convex polyhedra, sec Figure 5(a). 

2. A polyhedral model of the manipulator — represented as a set of link bodies connected by 
rotary or prismatic joints, sec Figure 5(b). 


3. A kinematic model of the manipulator — currently, partly embedded in procedures which 
apply to the polyhedral model and partly in the model structure. 

4. A start and a goal configuration for the manipulator. 


The system output is a safe path from the start to die goal configurations of the manipulator. The 
paths are composed of a sequence of linear segments in the Cspace of the manipulator. 


The implementation described here is limited to cartesian manipulators, i.c. those having three 
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perpendicular translational degrees of freedom corresponding to the x, y and z axes and up to three 
rotary degrees of freedom, usually centered at the wrist. Figure 6 illustrates two different types of 
cartesian manipulators. The restriction to cartesian manipulators allows the use of the CO% JZ (B) 
algorithm described in Section 3.2 as the main tool for capturing path constraints. 

The Findpath algorithm carries out the following steps in turn: 

1. Constructing the Capace, \ obstacles — The slice projections of the CspaceA obstacles 
approximate the constraints on the configurations of the manipulator due to the presence of 
objects in the manipulator’s workspace, see Section 5. 

2. Representing free space — Once the CspaceA obstacles are known, the system computes 
a decomposition of the space outside these obstacles into convex polyhedral cells; these 
polyhedra arc then linked into a graph structure called the Free Space Graph. Each node of the 
graph represents a free space cell and a link between cells indicates that they touch or overlap, 
sec Section 6. 

3. Searching for a safe path — 'flic Free Space Graph is searched to locate a cell path , a 
connected set of free space cells that join the origin and the destination. From the cell path, the 
system derives a line path, a piecewise linear path in the manipulator’s Cspace, see Section 7. 


5. Computing the CspaceA Obstacles 

The first and most important step in the Findpath algorithm is that of computing the CspaceA 
obstacles arising from the presence of objects in the workspace. The CspaceA currently used by the 
system is the seven dimensional joint space of the manipulator, i.e. x, y, and z displacements, the 
three wrist rotations and the finger opening. The CspaceA obstacles are complicated objects in this 
high-dimension space. To avoid having to deal directly with these objects, the system makes use of 
slice projection to approximate the CspaceA obstacles by a set of three-dimensional obstacles. 

The CO% /z (B) algorithm of Section 3.2 computes an (a;, y, z) cross-section of CO A (B) for a 
specified orientation of A. But, this algorithm can be adapted to compute the (x, y, z)-slice projections 
of CO a {B). The construct that relates slice projections to the cross-sections is the swept volume of 
an object. The swept volume of A is the union of (/4) 0 , i.e. A in configuration a, for a within the 
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configuration range denoted by [c, d\x, where c and d arc configurations of A and K is a subset of 
die configuration parameters. A configuration a is in the range [c, c']/< if, for each i in K, the i th 
parameter of a is between the i lh parameters of c and d. For example, if c and d are of the form 
(/?and K — {3}, dicn the swept volume of A over the range [ c,d]K refers to die union of 
A over a set of configurations differing only on /?j. The swept volume of A over this configuration 
range is denoted A\c, d}^. It can be shown [27] that the {x, y, z)-slice projection of COa{B ) over the 
orientation range contained in [c, d] K is the same as COJM* «(B). 

In summary, the computational requirements of the slice projection technique are: 

1. Choosing a decomposition of the orientation ranges of die cartesian manipulator into sub¬ 
ranges, [c, d]K, to be used for slice projection. 

2. Computing polyhedral approximations to A[c, d]h for each orientation range. 

3. Computing COT^, (Bj) for each obstacle Bj and each orientation range. 


This section addresses these issues. First we assume that the orientation ranges defining die slices are 
given; the discussion of choosing slice parameters will be taken up at the end of the section. 


5. /. Computing the Swept Volume for Linked Polyhedra 

The swept volume of a polyhedron A over a range of translations is another polyhedron. Let 
T C I be the set of configuration parameters corresponding to the translations of A. HA is a convex 
polyhedron and die range of positions of the reference vertex of A over the range of translations 
[c,d\K can be represented as a convex polyhedron V , then A[c, d\r — A ® V where X 0 Y — 
{ x -f- y | x G X and y G Y }. Since A ® V = conv(vert(A) ® vert(V)), this leads to a direct 
algoridim for commuting the swept volume for translation. If die range of configurations includes 
rotations dien the swept volume is not a polyhedron. In die rest of the paper it is assumed that 
a polyhedral approximation to the swept volume is always available. The Appendix describes an 
algorithm to compute a simple approximation to the swept volume of a convex polyhedron under 
pure rotation. 

The swept volume of A , a rigid object, resembles another rigid object with the same number 
of degrees of freedom. But for manipulators, modelled as linked polyhedra, the situation is more 



Figure 7. Changes in the second joint angle from 0 2 to 0 2 causes changes in the configurations of both link 
A 2 and link A 3 . 


complex. Linked polyhedra are kinematic chains with polyhedral links and prismatic or rotary joints. 
The relative position and orientation of adjacent links, A t and A i+l , is determined by the i lh joint 
parameter (angle) [36]. The set of joint parameters of a linked polyhedron completely specifics the 
position and orientation of all die links. 

Note that for a linked polyhedron, the position of link j typically depends on the positions of 
links/e < j, which arc closer to the base than link j. Let K — {j}, c — (0;), d — (0'), and [c, d]i< 
define a range of configurations differing on the j lh Cspace.\ parameter. Since joint j varies over a 
range of values, links7 > j will move over a range of positions which depend on die values of c and 
c', as shown in Figure 7. The union of each of the link volumes over its specified range of positions is 
die swept volume of the linked polyhedron. The swept volume of links j through n can be taken as 
defining a new j th link. The first j — 1 links and die new j lh link define a new manipulator whose 
configuration can be described by the first j — 1 joint parameters. On die other hand, die shape of the 
new link j depends not only on the /7-parameters of c and d, i.e. 0; and 0' , but also on 0/ for l > j. 
This implicit dependence on parameters of c and d that are not in K is undesirable, since it means 
diat the shape of the new j lh link will vary. Letting K = {j ,..., n.}, then die shape of the swept 
volume depends only on the K -parameters of c and d, while its configuration is determined by the 
(J — /7)-parameters. A swept volume that satisfies this property is called displaceable. 




15 


Computing the Csjxice^ Obstacles 



Figure 8. Computing the swept volume for linked polyhedra. If [c, r'j/v involves ranges of configurations of 
the second and third link, first compute the swept volume for the third link and then the swept volume for 
the union of the second link and the swept volume of the third link. 


Given an operation for computing (a polyhedral approximation to) the swept volume of a 
polyhedron, see Appendix, then this operation is applied to computing the swept volume of linked 
polyhedra. The swept volume A [c, d]x is computed by die following process, illustrated in Figure 8: 

1. Let i = n, where n > 0 is die number of links in die linked polyhedra, |/| == n; let 
A* = 0; 

2. Place A in configuration c; 

3. Let A* = A*KjAi\ 

4. If i € K then let A* = A*[c, c 7 ]^}, i.e. update A* to be the swept volume of A* over the 
range of i lh joint; 

5. Let * = i — 1. If * = 0 then stop, else go to step 3. 
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The swept volume obtained in this fashion can then be used to compute the CO 


5.2. Com puling slice projections for C spacer obstacles 


If A\c,c% overlaps some obstacle B then, for some configuration o in the range [c, c'] K , (A) a 
overlaps D. The converse is also true. If A\c, c’}i< is displaceable, dien C0 A [ cc ^ h (B) is the set of 
/ — K projections of those configurations of A within [c, c^v for which A overlaps B. Hquivalcntly, 
CO A i c yi K (B) is the I — K projection of the [c, c']^ slice of CO A (B). HA is a cartesian manipulator 
and K is the index set for the wrist rotations of the manipulator, then the configurations of the swept 
volume arc the (x, y, z ) positions of some point on the manipulator. The algorithm of section 3.2 can 
be used to compute CO x kj c ,^{B) and thereby compute the required slice projections of CO A (B). 

Given the swept volume of die manipulator model for a particular range of parameters [c, c5]k, 
the next step is to compute the slices of all die Cspace obstacles for die manipulator over that range 
of configurations; this set is denoted COS[c, <f\. In previous discussions of the CO" J '(B) algorithm 
we have assumed that A and B were single polyhedra; we saw in the previous sections diat both 
the object and manipulator models arc structured as part trees, whose leaves arc convex polyhedra. 
The actual model of a manipulator or a part is die union of die fringe, i.c. the set of leaves, of the 


corresponding part tree. Thus if >4 
in computing COjf z (B): 


Ujl,^ and B — Uj L i Bj, die following result can be used 


| kn 
'j 


Ica ks 

CO a (B) = U U co *SPil 

»=i J=l 


This result means that k A x k Li applications of die CO x f z {Bj) algorithm must be carried out to 
compute CO x J[ lz {B) exactly. In die "pick and place" application, an exact model of all the Cspace 
obstacles is not usually needed since the manipulator will not move close enough to all the obstacles. 

The amount of time needed to compute the COS can be reduced by simplifying the geometric 
models of both the Ai and die By when appropriate. The current implementation uses a simple family 
of succcsivcly finer approximations for objects based on the part tree. Consider the part tree for 
an object Bj, where each of the leaves of the tree is a convex polyhedron. Define a covering node 
set recursively to be eitJicr (1) the set containing just the root of the part tree or (2) obtained from 
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another covering node set by replacing some node, internal to the part tree, with all its descendants. If 
each internal node represents the union of all its descendants, then every covering node set is a com¬ 
plete model of the object. In practice, internal nodes of the part tree store the bounding rectangular 
solid 5 of the union of all its descendants instead of die union itself. Thus, the family of covering 
node sets represents progressively more detailed models of die part [29]. Using these approximations 
reduces the number of applications of CO^'~(Dj) needed to compute die COS , since die number 
of polyhedra in a covering node set is less than or equal to that in the full fringe. In addition, it 
can be used to simplify many of the individual computations, because when A and D are bounding 
rectangular solids, computing CO^ yz (B) is trivial. In particular, if die bounding solids arc represented 
by die endpoints of their main diagonal, e.g. A = ( 01 , 02 ) and B — ( 61 , 62 ), dien CO^ yz (B) = 
(6| — (02 — oi), 62)- 


For simplicity, die current implementation uses a diree level part tree for the swept volume of 
the manipulator and for the objects in the workspace. Each tree has a root node which models the 
complete object by one bounding rectangular solid. The descendants of the root arc bounding rectan¬ 
gular solids for each of die convex components of the model and die leaves of die tree arc the convex 
polyhedra whose union is the complete object model. Therefore if the object is modelled as the union 
of A; convex polyhedra, the part tree has 2kl nodes. Using this representation, CO% JZ (B) can be 
modelled as a tree of similar structure with 2(k A X kn) -|- 1 nodes. Any covering node set of diis 
tree is an approximation to the Cspace,\ obstacle corresponding to B. In practice, the complete tree is 
not computed at once, rather the simplest approximation, the bounding rectangular solid of the whole 
object, is computed and successive covering node sets are computed as needed. This is discussed 
further in section 6 . 

5.3. Choosing the Slice Parameters 


So far we have assumed that the configuration ranges defining the Cspace A slices were given as 
input; in this section, die choice of ranges is discussed. 'Hie primary choice is how large to make the 
ranges, since it is this that affects the system’s capability to use changes in die orientation of die hand 


r A bounding rectangular solid for a polyhedron is a rectangular solid whose edges arc parallel to the coordinate axes 
and that completely includes the polyhedron. 
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to avoid obstacles. In particular: 

1 . The larger the orientation range of a slice, the larger the manipulator’s swept volume, the 
larger (and less accurate) the CspaceA obstacles and the fewer the legal configurations and legal 
motions of the manipulator. 

2. The smaller the orientation range of slices, the larger the number of slices needed to cover 
die Cspace and the more time needed to compute the COS and to search them for a path. 

These conflicting effects can be balanced by taking advantage of the fact that, for "pick and place" 
motions, the accuracy requirements are higher near the start and the goal of the path, where the 
manipulator is moving near obstacles, than along the rest of the path [28] [48] [49]. This suggests 
defining slices with small rotation ranges centered around the orientations of die start and the goal; 
slices with larger ranges may be used for the remaining orientations. This approach is used in the 
current implementation. In particular, a COS is defined for die orientation of the manipulator in 
the start configuration and one for the orientation manipulator in the goal configuration; diese COS 
correspond to slices with singular orientation ranges, i.c. where die upper bound of the range equals 
the lower bound 6 . In addition, the total range of parameters in Cspace A is divided among some 
number of other slices 7 each with non-singular ranges. Furthermore, slices with singular ranges are 
defined for configurations at the intersection of the slice parameters of die "larger" slices. This last 
type of slice allows moving between safe configurations in the "larger" slices. 

Note diat the computational burden of adding an extra slice is very low if bounding rectangles 
arc used for objects. This sacrifices some of die potential maneuvering space, but gains a very large 
increase in speed. This is the compromise taken in the current implementation. 

Motions within a slice with a singular orientation range arc limited to translations, while rotation 
is legal within a slice with non-singular ranges. Therefore, the classes of motions allowed by the 
system arc those composed of translations interspersed with rotations, but where die rotations happen 
in increments defined by the slices parameters. Ihis means that this approach may fail to find a safe 
path in situations where: 


6 A slice with a singular range is the same as a cross-section. 
Currently varying between 8 and 64. 
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1 . all safe paths require rotations combined with translations at a finer resolution than that 
allowed by the slice ranges, and/or 

2 , the orientation ranges chosen, although adequate in si/.c, do no match those required in the 
problem. 

These problems can be reduced, at the expense of more computation, by using more slices with 
smaller ranges. But, there exists problems which require continuous rotation along a path. In practice, 
most robotics applications do not use the very crowded environments that require very high rotadon 
resolution for die "pick and place" motions. The reason for this is that safe paths in such environ¬ 
ments are very hard for humans to specify, are subject to positioning errors of die parts and are 
difficult for most industrial robots to execute reliably at medium or high speeds. 


6. Path Searching and Free Space 


I laving computed the Cspace A obstacles, it still remains for die system to find a path among these 
obstacles. 1’his section briefly touches on alternative strategics for finding safe paths. 

One approach to finding paths among obstacles is to search for die shortest path between die start 
and the goal, widiout considering other constraints. For example, the Vgraph algorithm described in 
Section 3 follows diis approach. But, the approach has some important drawbacks. Shortest padis in 
Cspace A move along the boundaries of die Cspace A obstacles and are, therefore, very suscepdble 
to model inaccuracy and position error. This problem can be alleviated by adding a uniform "safety 
margin" around the obstacles, but doing so might disqualify some feasible paths. Furthermore, 
no efficient algoridims currently exist for finding optimal padis among three-dimensional obstacles. 
Unlike the situation in two dimensions, there is no finite set of points through which shortest paths 
arc guaranteed to pass. Tlius, algorithms would have to be based on iterative numerical mcdiods. For 
these reasons, only heuristic algorithms for finding safe paths will be considered here. These heurisde 
algorithms require less execution time and can be extended to consider criteria such as safety margins, 
but they will not find die shortest path. 

Another issue is whether die padi search is conducted using primarily a representation of the 
Cspace A obstacles themselves, as does the Vgraph algoridim, or of the free space outside the 
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obstacles, as in [48] [49]. Although these representations arc equivalent, they lead to different heuristic 
algorithms. The current implementation uses the free space style of algorithm because it simplifies 
the formulation of different search heuristics, c.g. the use of variable resolution space representations 
described below. 

The remainder of the section deals with the free space representation technique employed in the 
Findpath implementation. Section 7 discusses the path search algorithm used on this representation. 


6.1. A Free Space Representation 


The basic goals for a space representation are accuracy, speed and compactness. In addition, 
it should facilitate heuristics for the task at hand. The most important heuristic for a space repre¬ 
sentation is to avoid excess detail (and therefore time spent) on parts of the space which do not affect 
the operation. 'Therefore, the space representation should not have to maintain a perfectly detailed 
model everywhere. Instead, it should have the capability of maintaining a rough model and be able to 
selectively refine [48] [49] subsections to be as detailed as necessary. 

A number of proposals exist for representations of space and objects in space [9] [25] [42]; most 
of these divide the space into a set of cells. The proposals can be partially characterized along the 
following dimensions; 

1. Shape uniformity — are all cells equally shaped? 


2. Size uniformity — arc all cells the same size? 

3. Orientation uniformity — are all cells oriented uniformly? 

4. Ordering principle — arc the cells ordered into an array, multi-list, tree, or graph. 


We will not consider representations which use cells of uniform shape and/or size, since they typically 
require large numbers of cells to achieve sufficient accuracy 8 . Instead, we use a hybrid cell repre¬ 
sentation employing two types of cells: (1) rectangular solids aligned with the axes and (2) arbitrary 
convex polyhedra. The idea is to use the simple rectangular cells away from obstacles where repre¬ 
sentation economy is important and polyhedral cells where high accuracy, e.g. near an obstacle, is 

8 Udupa [48] [49] employed a free space representation which used rectangular cells of variable size. This approach is 
adequate for motions that do not closely approach the obstacles. 



21 


Path Searching and Free Space 



Figure 9. This figure illustrates, in two dimensions, the space representation employed in the implementation 
of the Findpath algorithm, (a) A sample Cspace A obstacle with its part representation, (b) The resulting 
space representation. Rectangular nodes indicate mixed cells, round nodes indicate full cells, and triangular 
nodes indicate empty cells. 


needed. 


The space representation described below is analogous to the part representation described 
earlier, except that a new type of node is introduced, 'rhe part tree representation uses rectangular 
bounding cells as internal nodes and polyhedral cells as leaves. The leaves represent space that is 
FULL, i.c. completely occupied by an object. The internal cells represent MIXED space, i.c. cells 
which arc part FULL, part EMPTY. But, note that the part tree docs not have an explicit repre¬ 
sentation of die EMPTY space. The space representation simply adds explicit EMPTY cells to the 
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parts tree representation. Then each internal MIXED node becomes tlie union of its descendants. 
In addition, the space representation introduces a new MIXHD root node from which all the part 
representations descend. 

The space representation is built up starting with a bounding rectangular solid representing die 
workspace, this is die first MIXED cell. The descendants of diis node arc the MIXED cells cor¬ 
responding to the roots of the trees representing each of the CO" r (Bj), as described in Section 5.2 
and a set of EMPTY bounding rectangular solids representing die free space outside the MIXED 
cells. The representation of each MIXF’D cell can be further expanded into other EMPTY, MIXED 
and FULL cells, culminating in a representation involving only EMPTY and FULL convex polyhedral 
cells as leaves of the tree and MIXED cells as internal nodes. Figure 9. The polyhedral representation 
of each EMPTY cell must be computed so that it does not overlap any MIXED or FULL cells. As 
with the part representation, any covering node set of this tree represents a complete model of the 
space, at some non-uniform resolution. This hybrid cell representation is based on a generalization 
of the quad tree representation used for images [8] [17] [18] [20] [43] and the oct-tree representation of 
objects [3]. 

The operations on die space representation described above arc very efficient when dealing with 
bounding rectangular solids. The most expensive operation is when the volume difference of a 
MIXED rectangular cell and a FULL polyhedral cells must be computed 9 ; this operation results in 
a description of die EMPTY cells. However, this need only be done when high accuracy is required, 
usually near the start and the goal of die path. Therefore, die representation meets the criteria stated 
at the beginning of the section. 


6.2. Building a Free Space Graph 

The process described in Section 5 produces a slice for each Cspacer obstacle over each of the 
orientation ranges, [c t , c']/<, of the manipulator’s wrist. 'The set of slices for all obstacles over one 
orientation range is denoted COS\ci, c']/< . For each of these COSi, a space representation is com¬ 
puted, SRi, as described above. For each of these SRi, a Free Space Graph is built, FSGu this is 
a graph where each node is an EMPTY cell in the SRi and a link indicates that the cells touch or 

’The current implementation of this operation uses repeated applications of a cutting and capping operation [6]. 
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overlap 10 . In addition, ii is necessary to add links to each FSG X that connect to nodes of other FSGj 
whose rotation range overlaps that of FSGi. That is, for EMPTY cells C l E SR l and Cj E SRj, if 
there is some configuration c contained in both cells, then links must be placed between C x and Cj. 
This is so because the existence of c guarantees that it is possible to pass from any configuration in C x 
to any in Cj and viceversa while remaining outside all the obstacles in COS, and COSj. The resulting 
composite FSG is then searched for a path, since each path through the graph corresponds to a class 
of safe paths in Cspace 4 and vice-versa. 


7. Path Searching 

The Findpath problem is to find a path between two points, the start and the goal, while staying in 
the free space. In the current implementation, this is carried out by the following steps: 

1. Choose die largest EMFIT cell in any of the S/2, enclosing die start configuration. 
Otherwise, choose some MIXED cell containing the start and expand the representation of 
diis MIXED cell into its constituent EMPTY, MIXED and/or FULL cells. If an EMFIT cell 
contains the start configuration, stop, else repeat. Note dial this computes successively finer 
models, i.c. successive covering node sets, of the specific area around the start without having 
to expand the complete model or even any complete part tree. If no EMPTY cell is ever found, 
die task is impossible since the start configuration causes a collision. 

2. Perform step 1 for die goal configuration. 

3. Construct a Free Space Graph as described in Section 6 . 2 . At this point, the Free Space 
Graph is in its final form; the current implementation does not refine the space representation 
further. 


10 The current representation allows EMPTY cells to overlap each other but not MIXED or FULL cells. 
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4. Search for the shortest path in the Free Space Graph from the cell including the start to 
that including the goal. The graph search operation can be carried out by any of the standard 
shortest path algorithms [13]; the current implementation uses the A* algorithm [15]. These 
shortest path algorithms require that a weight be assigned to each of the links of the Free Space 
Graph e.g. indicating the time required to traverse the cells. How this may be done is discussed 
below. If no path exists, this may be due to the approximations and quantizations used in the 
solution, see Section 7.3. 

5. Choose a line path contained in the cell path. This problem is discussed in Section 7.2. 


7.1. Assigning!.ink Weights for the FSG 

The definition of an "optimal" path, or even a "good" path, assumes some choice of performance 
index, 'flic current implementation uses estimated time of travel along the path as die index. If 
Cspace A is the manipulator’s joint space, then the time to travel between two configurations can be 
estimated as the maximum time for any of the joints to travel, at the maximum rated joint velocity, 
between the joint settings at each configuration. The weights assigned to the links in die FSG should 
therefore reflect the time needed to travel between two overlapping cells along the optimal path. Of 
course, no weight assignment can actually do this since it requires knowing die complete optimal path. 

A simple alternative is to assign to a link die estimated time of travel between die centroids of the 
cells that it connects. This weighting function has the advantage of being very easy to compute. For 
small cells it provides a good approximation of the actual time to traverse the cells, but for larger cells 
it might overestimate or underestimate the actual time, see Figure 10. The current implementation 
uses the centroid weighting function, but docs not divide the large EMPTY rectangular cells into 
smaller cells; this will be implemented in the near future. 

A frtbfe complex weighting function, which would typically produce faster paths, is the following: 
The weight on the link between cell C and C' is assigned the time to traverse C from p, the point 
of entry to C, to p', die point of entry into C'. The point p' is the one on C n C that minimizes 
die distance 11 to the line between p and the goal. The initial C is die cell that contains the start 

11 Actually, the difference in time between the straight line path and one going through this point. 
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configuration and the initial p is the start configuration. Clearly, this technique requires much more 
computation that the centroid weighting described above. For most applications, the simpler centroid 
function, together with cell splitting should suffice. 


7.2. Choosing a Line Path 


The search of the FSG produces a list of EMPTY C space A cells that touch or overlap; it is still 
necessary to choose a specific path, i.e. some curve, within these cells. The simplest type of path 
to choose is a piecewise linear one, although the cells simply place configuration constraints on the 


manipulator along the path and any path 




If the centroid weighting has been used for the links, it is natural to choose a piecewise linear 
path that traverses the centroids of the cells. Of course, the straight line path between two centroids is 
not guaranteed to remain within the cells and might therefore not be safe. Therefore an intermediate 
configuration in the intersection between adjacent cells should be chosen. The centroid of the inter¬ 
section of adjacent cells on the path can be used for this purpose; this is the technique used in the 
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current implementation. Alternatively, this point could be chosen so as to minimize the deviation 
from a straight line path between the centroids. If the cell size is small enough, such paths are 
adequate for most tasks. 

The more complex weighting scheme described earlier produces a sequence of entry points into 
the cells which may be connected directly to obtain a path. Since the points arc contained in the 
intersection of the cells, a straight line connecting them is guaranted to be in the cell. 


7.3. Dealing with Path Search Failure 

If the path search algorithm fails to find a safe path, the reason for failure could be one of the 
following: 

1. No safe paths exist. 

2. No safe paths exist at die quantization of orientations chosen. 

3. The approximations of objects by bounding rectangular solids has removed necessary 
maneuvering space. 

The last two causes of failure may be overcome by decreasing the orientation quantization and/or 
increasing the representation detail in the space representation, both at the expense of extra computa¬ 
tion. This suggests the possibility of increasing the accuracy of the space representation when a path 
search failure ocurrs. The current implementation does not exploit this possibility. 

8. Examples 

This section presents output from the implementation running on a simple example. The results 
are collected in Figure 11. 

a. The initial and final configuration of the model, including the manipulator model. Note 
that the manipulator must rotate to execute this motion. 






Figure 11a 

The start and goal configurations and 
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Figure lib 

START COS: The Cspace obstacles for the manipulator in the 

start configuration. 
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Figure lid 

The Cspace obstacles for the swept volume of the manipulator 
over a range of configurations of the wrist. 
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Figure Ilf 

The Cell Path and Line Path superimposed on GOAL COS 
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b. The COS for the start configuration. Kach convex solid in the figure is a representation of 
CO^ z (Bj). Note that most of these Cspace.\ obstacles are rectangular solids, except for those 
arising from the interaction of the hand, A, j, with block B\ and the fingers, A\ and A iy with 
the table. In these cases, the manipulator is so close to these obstacles that its configuration is 
inside the bounding rectangular solid for the configuration obstacles (In practice, the sides of 
the bounding rectangular solid are displaced outward by some small e). This condition causes a 
detailed expansion to be carried out. 

c. The COS for the goal configuration. In the goal configuration none of the obstacles needs 
to be expanded in detail. 

d. The COS for one of the intermediate configuration ranges. This COS is defined for the 
manipulator’s swept volume over a range of orientations of the wrist and hand. One bounding 
rectangular solid, A\, approximates the swept volume of the hand and fingers, A { U A% U Aj. 
The solids A 4 and /I 5 remain unchanged. 

e. The cell path and the line path. This shows the cells from the various space representations 
that compose the cell path. One group of cells correspond to free space for the initial 
configuration, one large cell comes from the intermediate configuration (where the hand rota¬ 
tion takes place), and the last group of cells correspond to the final configuration. The line 
path shown goes through the centroid of each of the cells and also through the centroids of 
die intersection of adjacent cells on the path. Notice that because the cells are large, this path 
strategy produces paths that move too far from the obstacles. This could be overcome by sub¬ 
dividing the cells before finding the line path. 

f. The cell path superimposed on the start COS. This shows the relative placing of the free 
cells relative to the obstacles. 

9. Choosing Crasp Configurations 


The preceding sections have discussed die problem of finding sale paths for the manipulator; this 
only part of the "pick and place" synthesis problem. The major remaining problem is choosing 





29 


Choosing Grasp Configurations 


a grasp configuration on the part, P. For simple parts and non-chittcrcd environments, grasping is 
amenable to simple ad-hoc solutions. As a step in die solution of this problem, we deal here with 
choosing grasping configurations for relatively simple parts in cluttered environments. In this section, 
a Cspace approach to this problem is described, although no implementation of this approach to 
grasping currently exists. 


The grasping problem is related to the Findspacc problem introduced in Section 3, insofar as it 
involves choosing a safe configuration among a set of obstacles. But, there arc additional constraints 
on the choice, for example: 

1 . the manipulator’s fingers must be in contact with P , 

2 . the configuration must be reachable, and 

3. P must be stable in the manipulator’s hand, i.e. it will not slip in the hand during a motion. 


'The first two conditions, contact and reachability, reflect additional geometric constraints on the solu¬ 
tion to the Findspacc problem. The third condition, stability, reflects aspects of grasping beyond the 
purely geometric. Stability will be briefly discussed later in the section. 


The approach to grasping described here is based on the one described in [25] and [26]. The 
basic idea is to build an explicit description of the set of configurations of the manipulator A for 
which the inside of the manipulator’s fingers are in contact with specified surfaces of P. This set 
of configurations is some subset of CO A {P), call it G. Feasible grasp configurations are those in 
G, that do not cause any collisions with other objects in the workspace, i.e. that are outside all of 
the CO A (Bj). In this section, the details of this approach are discussed. We make the following 
simplifying assumptions: 

1. The manipulator is cartesian and its hand is a parallel jaw, i.e. two parallel fingers that move 
along their common normal. 

2. Only parallel planar surfaces, whose distance from each other is less than the maximum 
finger opening, arc candidates for grasping. These are known as grasp surfaces. 

These assumptions simplify the method for identifying feasible grasp configurations, while suggesting 
its usefulness and providing the foundation for a more general approach. 
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Figure 12. The definitions of Pi, Pj, Fi , F^, and II used in choosing grasping configurations. 


9.1. Feasible Grasp Configurations 

Let Pi and Pj be the parallel faces 12 of P to be grasped, and Fy and F 2 be the inside faces of 
the manipulator’s fingers, Figure 12. Under the two assumptions suited above, when A grasps P, Fi 
and F '2 are coplanar with P, and Pj respectively. Under these conditions, the legal ( x, y,z) positions 
of rv A are restricted to some plane H diat is parallel to Pi and Pj. Let G A (Pi, Pj) be the set of 
configurations of A for which rv A is in H and for which P,, Pj, Fi, and P 2 are mutually parallel. Note 

i _ 

that G A (P i) Pj) represents those positions and orientations where A could be when grasping P t and 

Pj, without specifying the distance between the fingers. G A (P{ } Pj) is called the grasp set for Pi and 

p. 

r v 

Note that not all the configurations in G A {P{, Pj) are feasible grasp configurations, cither because 

12 Note that objects in the current implementation are modelled as unions of convex polyhedra. Convex polyhedra are 
defined as the intersection of a finite number of half-spaces, where each half-space is bounded by a plane. The portion 
of each bounding plane on the boundary of the polyhedron is a convex polygon, known as a face of the object 
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the fingers are not in contact with the grasp surfaces or because the manipulator configuration causes a 
collision with some other object. Therefore, we must impose two additional restrictions: 

1 . The internal faces of the fingers must overlap the grasp surfaces. 

2. The manipulator must not collide with any other object in the workspace, i.e. the Bj. 

With these restrictions on the configurations in the grasp set, we obtain the set of feasible grasp 
configurations, called a feasible grasp set and denoted FG A (Pi, Pj). 

Define die configurations of Ft and Fz to correspond to those of the manipulator, i.e. each 
position and orientation of these faces is characterized by the manipulator configuration which would 
place them there. From these definitions it follows that CO/.-,(P<) is the set of those configurations of 
A for which the Fi is in contact with Pj. Furthermore, COrfPi) C\G A (Pi, Pj) are those configurations 
for which the finger is in surface-surface contact with Pi. Therefore, it follows that 

FG A (Pi, Pj) = (co Fl (Pi) r)CO,4Pj) n g a (p u p, )) - |J co a (Bj) 

In this definition, wc must let P be one of the Bj , say B p , so as to avoid collisions with P while 
approaching a grasp configuration, but we must also allow A to contact P on die grasp surfaces. The 
answer is to add a slight displacement inward to Pi and Pj, when computing CO A (B p ), while using 
the original definition in die computation of CO Fl (Pi) and COp 2 {Pj). 

The feasible grasp set, as defined above, is a volume in a six-dimensional Cspace A . Wc do not 
have algorithms for computing this volume exactly. T he algorithms of Section 3 serve only to com¬ 
pute slice projections of die Cspace A obstacles. It is clear that die same must be done for the feasible 
grasp set, namely computing its slice projection for some range of orientations. Such a slice would be 
the set of (x, y,z) positions of A that, for some range of orientations of A, arc in contact with P, but 
outside all of die Bj. Presumably, diis requires using die slice projections of COpfPi), GOpJJPj), and 
the CO A (Bj). A problem arises when trying to do this, because slice projections were defined over 
simple orientation ranges of the cartesian manipulator’s wrist defined in Section 5. These ranges are 
not, in general compatible with the ranges of orientations that define G A (Pi, Pj). For a position of 
rv A oh H, only a small range of orientations will result in configurations that are in G A (Pj, Pj), yet for 
that position to be in a slice of FG A (Pi, Pj) it must be the case that no orientation within the slice’s 
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defining range causes a collision. Therefore, few, if any, configurations in the grasp set will be feasible 
grasp configurations. 

The solution to this problem is simply to define a new set of slices whose orientation ranges are 
subsets of the orientation ranges in G A (Pi, Pj). Note that a configuration in such a slice already 
satisfies the orientation constraints of the grasp set. Therefore, only the position constraints, i.e. that 
die (x, y, z) position be in ft, need to be enforced to obtain the intersection of a Cspace obstacle in 
that slice with the grasp set. This removes the need of computing tire complete representation of the 
obstacles, while simultaneously avoiding the problems introduced by irrelevant orientations. 

Computing the obstacle slices for orientations in the grasp set requires being able to compute the 
swept volume of the manipulator over orientation ranges that are not the simple ranges of joint angles 
defined in Section 5. Let R be the set of orientations in the grasp set that define a slice and denote 
the swept volume of A over R as /l [ft]. Algorithms for approximating the swept volume over these 
ranges can be based on the simple approach described in the Appendix. The important constraint on 
the approximation to [ft] is that it docs not intersect the grasp surfaces for positions of rv A on ft. 

In addition to the manipulator displacing and rotating, the manipulator’s fingers may move per¬ 
pendicular to the grasp surfaces. This additional degree of freedom has not been discussed above. In 
fact, it poses no additional problems; the motion of the fingers can be treated, via slice projection, 

uniformly with rotation. 'Phis simply requires including the space swept out by the lingers during 

1 * ' ■' 

closing, in the swept volume used to define slices of die CO A (Bj). 

9.2. Overlap of Finger and Surface 

The approach described above deals adequately with the CO A (Bf] in the definition of feasible 
grasp set, but is less succcsfiil in dealing with CO/^P,) and CO F . 2 [Pj). The reason for this is that 
a position in the slice projection of CO Fi (Pi) simply indicates that for some orientation of A in the 
slice, the finger is in contact with P,. What is required instead is the set of positions which for §U 
orientations of A in the slice, there is contact. In fact, we would like to guarantee that the area of 
contact between the fingers and the grasp faces always exceeds some fixed area. Mow this may be 
accomplished is discussed below. 
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Tk 



b 


Figure 13. Defining the configurations of A for which F overlaps P. (a) Illustration of the definition of T k 
and S. (b) Illustration of CI%F z (P t )QS, with two positions of F k 's reference vertex (indicated by the small 

circles) showing the area of overlap includes an area of the form 7^08, for some 


Let F fc and Pi be, respectively, a finger surface and the corresponding grasp surface. We define 
T k to be a small strip at the tip of F k , such that F k = T k © S , where S is the set of points along a 
line segment, as shown in Figure 13. Again, we assume that the configurations of T k correspond to 
those of H (and therefore A). Assume A is in some configuration c € G A (P i} Pj), so that F k and Pi 
arc coplanar, then CIjF(Pi) is the set of (x, y,z) configurations of T k , and therefore of F k and A, 
for which F k C\Pi 2 T k . But, we do not want to restrict the overlap between F k and Pi to be at the 
fingertip; instead, we want the area of overlap to include some area T k , obtainable by translating T k 
along S, i.e. T' k — T k © {«}, with a G S. It is easy to show that 
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C7ff (P,) 0 S = { c | 3s G 5 : P t O ( F k ) c D ( T k ) c © {s} } 

Therefore, this is the desired set of configurations, see Figure 13. This result can be applied to com- 
pijtc t|ic specs needed for die feasible grasp set. If R is the orientation range defining the slice, then 
CI^l JPi) © S[R] represents the set of (x, y, z) configurations that, for orientations in R , guarantee 
that the contact between F k and Pt includes T k . Note that this approach can be generalized to any S 
and Tk such thatFfc = Tk © S\ as Tk becomes smaller and approaches a point, then S approaches F&. 

9.3. Safety at the Destination 

So far, the definition of FG A {Pi, Pj ) only embodies constraints relating to safety at P's initial 
configuration, however a grasp configuration must also be safe at P's final configuration. Clearly, 
another feasible grasp set can be computed at P's final configuration, say FG A (F it P') where the 
primed faces indicate the faces at their final configuration. But, these two feasible grasp sets cannot 
be intersected to obtain those grasp configurations that are safe for both configurations of P, be¬ 
cause a grasp configuration corresponds to different manipulator configurations at each different 
configuration of P. What is needed is a way of defining those grasp configurations in P's initial 
configuration that would lead to a collision when P is in its final configuration, Figure 14. 

A grasp configuration establishes a fixed relationship between the fingers and the grasped part, 
P. Let the final configuration of P be obtained by a displaccmnct consisting of a translation t and a 
rotation r, indicated by A,r(F). Clearly, any set of positions X bears the same relationship to A |f -(P) 
as Dfr M bears to P. Therefore, if CO% ,z {Bj) is a set of positions of A which cause collisions at P's 
final configuration, then DfJ(CO^ vz (Bj)) represent infeasible grasp configurations. Figure 14. This 
result also holds for swept volumes of A, therefore it may be used to ensure safety at the destination in 
the definition of feasible grasp sets. 

9.4. Computing the Feasible Grasp Set 

The discussion in the preceding subsections is summarized in the following definition of feasible 
grasp set, for some range of orientation in the grasp set. We denote this orientation range as R, and 
IctP' denote the same orientation range as R relative to P. but at P’s destination. We also let ( t, r) be 




Figure 14. (a) A side view of a manipulator hand, composed of a finger and a "palm", hold¬ 
ing P at the initial and final configuration, (b) In the initial configuration, the shaded area 
represents C0^ 2 (P t ) — (J. CO x A ,/z (Bj), i.e. the feasible grasp configurations for A, considering 

only safety at the origin and letting T be a point, (c) The CO a vz (Bj) for the final configuration 

of A and P. (d) The shaded area represents COp> z (Pi) — (J. CO a jz (Bj) u Dr'(CO A yz (Bj}), 

* 

which is tlic feasible grasp set that takes into account safety at the destination. 
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the displacement between the initial and final configurations of P. Ihen, the feasible grasp set, for the 
orientation range R and displacement ( t, r ), is: 


rojfatft. Pi) - QPiflWPi) n c/|?f H| (P,)) e s[K]) - |J co^Bj) u^'( cojm,)) 



All of the elements in this definition can be computed using the CO xyz algorithm of Section 3.2 and a 
swept volume algorithm. 


9.J. Approach and Departure 

Configurations in the feasible grasp set, as defined above, arc guaranteed to be safe both at P's 
initial and final configuration. While these conditions are sufficient in most situations, they do not 
guarantee that the feasible grasp configurations can be used during a "pick and place" operation. For 
a feasible grasp configuration to be a legal grasp configuration, it must allow the manipulator to reach 
and depart P’s initial and final configurations. Summarizing, the following conditions must hold for a 
legal grasp configuration: 

1 . It must be possible to reach it from the initial configuration of the manipulator. 

2. It must be possible to remove P from its in itial configuration safely. 

3. It must be possible to reach the P’s final configuration with P held in the hand. 

4. It must be possible to withdraw the manipulator from P’s final configuration. 

The Findpath algorithm described in the preceeding sections can be extended to deal with 
the problem of choosing a grasping configuration that is reachable from the manipulator’s initial 
configuration. As we saw above, the feasible grasp configurations, over some range of orientations, are 
those within some specified volume of Cspacer, but outside the slice projections of suitably defined 
CspaceA obstacles. Hence, they arc equivalent to the slices, COS\c, c 7 ] of Section 6.2. Therefore, 
a free space representation for the feasible grasp configurations can be constructed and the resulting 
free cells linked in the Free Space Graph. The feasible grasp configurations for alternative grasp 
surfaces can also be linked into the graph. In the resulting FSG, any path from the cell containing 
the origin to a cell containing a feasible grasp configuration shows that this grasp configuration may 
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be readied from the origin. The path searching process must be modified to search for any cell 
which contains a suitable grasp configuration, rather than searching for a particular cell containing the 
destination. 

Similarly, departure from the origin and approach to the destination could be handled by testing 
whether the destination is reachable, using the FSG constructed as above. The difference is that now 
die hand is holding P, therefore the polyhedral description of P must be treated as if it were part of 
die manipulator. This requires adding a new set of Cspace A obstacles, arising from the interaction of 
P and the objects in the workspace, to die ones already computed for the manipulator. T his is entirely 

9 

analogous to modifying the description of the manipulator, which is already modelled as a union of 
convex solids. But, the geometric relationships between P and die A t arc determined by the grasp 
configuration, which has several degrees of freedom. The problem can be approached by treating 
these additional degrees of freedom, via slice projection, just as the wrist rotations were treated. This 
approach imposes a great cost in additional computation. A simpler, though less general, technique is 
to use heuristics in choosing a feasible grasp configuration and dien test, via the path search process, 
whether that grasp configuration permits departure. If it does not, a new configuration might be 
chosen and the process repeated. This approach would be not be adequate for very cluttered environ¬ 
ments or situations involving parts mating at die destination. In such environments an approach based 
on slice projection would also be susceptible to failure. Furdier research is needed in this area. 


9.6. Stability in Grasping 

We have thus far not considered the issue of stability of the feasible grasp point. An adequate 
treatment of stability in grasping is not yet available, although some promising approaches exist [5]. 
The techniques described in this section can be used to implement two simple grasping heuristics, 
which work adequately when (1) the manipulator hand is made up of rigid fingers, (2) die object to be 
grasped, P, is small relative to die manipulator hand and (3) parts mating effects arc ignored. The two 
heuristics are: 

1. Ensure at least a minimum contact area of the fingers with the grasp surfaces, 'fhe amount 
of overlap should depend on object properties such as weight and surface smoothness. 
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2 . The perpendicular projection of P's center of mass should be near to Fj n P% and F%C\Pj. 

The implementation of the contact area heuristic was discussed above. Section 9.2. The center 
of mass heuristic can be implemented by giving preference to grasp surfaces for which the center pf 
mass, projected onto the plane containing Pi, falls within P{ and similarly for Pj. Furthermore, for 
specified grasp surfaces, the choice among legal grasp configurations should minimize the distance of 
the prjcction of the center of mass to the area of overlap between finger and grasp surface. 

These heuristics, though adequate for many tasks, are not a substitute for a general theory of 
stability in grasping. This remains one of the most interesting open problems in robotics. 



10, The Effect of Uncertainty 

In die preceeding sections we have assumed that: 

1 . the configuration of all die objects is known exactly, and 

2 . the configuration of the manipulator can be controlled exactly. 

Both of these assumptions are only approximations to reality. In practice, configurations can only 
be known to within some uncertainty. Both of these sources of uncertainty affect what manipulator 
motions are safe. 


10 .1. Modelling Worst-Case Uncertainly in Cspacer 

In Cspace A , the two sources of uncertainty have similar effects, i.e. modifying the shape of the 
Cspace A obstacles. This section deals with techniques for taking into account these effects. The 
following notation is useful in the discussion. Let e = (/?,•) = (Pi, ■■ ■, p n ) £ 3i n and similarly, let 
configurations be ( 7 *) = ( 71 ,..., 7 n ) £ 5R n . The index set {1,..., n} will be referred to as /; let 
K C 7. The set £//<(e) denotes the set of configurations in C space A whose /C-parameters arc less than 
the absolute value of the corresponding parameter of e. 

~Pi < 1i <Pi, 

7 « = 0 , 


( 7 *) £ Ui<( e ) 


if* e K 

otherwise 






39 


Ihc lificct of Uncertainly 

Uncertainty in the configuration of A in Cspace A can be represented as a region around its 
nominal configuration, c; within this region are all the configurations that A may be in. Simple regions 
can be characterized by {c} ® Uh(e 4 ). Assume that (A)„ nB 7^ 0, i.e that a G COa(B). Any 
nominal configuration a' such that a' -f- x = a, for x G should also belong to COa{B). This 

means that under uncertainty of A, CO A {B ) should be replaced with CO A {B) 0 Ui^a). In practice, 
we do not ever compute CO A {B)\ rather, we compute slice projections of it using the swept volume 
of A over ranges of orientation parameters,/?. Therefore, the orientation and translation uncertainty 
must be treated separately. Orientation uncertainty affects the definition of the manipulator’s swept 
volume. For example, to compute a slice with parameters [c, o']/*, the swept volume A [c—e^, 
is used in place of A[c,c']n. The effect of the uncertainty in the translation parameters, T, can be 
computed as indicated in Section 5.1, using the CO x / f z {B ) algorithm. 

The worst-case effect on COa(Bj) of uncertainty in the configuration of the By, can be modelled 

» 

by replacing B with the swept volume of B over the uncertainty range. Alternatively, if the uncer¬ 
tainty in the configuration of By can be approximated by an uncertainty in translation 13 , Ur{eB,) then 
the uncertainty of A and B can be combined into a single uncertainty 14 and treated as die uncertainty 
of A. If T is the set of indices for translation parameters, then the combined uncertainty is: 

UtVa) = u tM © U r {e A ) 

10.2. The Effect of Uncertainly on "Pick and Place" Synthesis 

The presence of uncertainty significantly affects manipulator programming in general and the 
synthesis of "pick and place” motions in particular. One approach to planning motions in the 
presence of uncertainty is to plan paths that are safe under the worst case uncertainty, i.e. paths 
outside the expanded Cspace A obstacles defined above. This approach rules out most operations 
that involve moving near objects, e.g. grasping. Another approach is to assume that uncertainty 
does not significantly affect the outcome of most operations and to plan motions assuming nominal 

13 This can be done by defining a new translation uncertainty such that the swept volume over this range of positions 
will contain the swept volume over the original uncertainty range. 

14 'Hiis assumes that the translation space of the manipulator is the same as that of the objects in the workspace, which 
is true for cartesian manipulators. 
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configurations. A compromise position is to redefine the "pick and place" synthesis problem so as to 
isolate those operations that are most susceptible to uncertainty from those others where uncertainty 
plays a relatively minor role. The latter can be addressed by the techniques outlined in this paper, the 
former require a different approach. One possible re-definition of the "pick and place" problem is the 
following: 

1. Find a nominal grasp configuration assuming that there is no uncertainty. , 

2 . Identify a grasp approach configuration, a configuration that can be shown to be safe under 
worst-case uncertainty estimates for object and manipulator configuration. 

3 . Identify a grasp deproach configuration, a configuration which is safe for the manipulator 
grasping the part, given the uncertainty in the part’s configuration after grasping and the uncer¬ 
tainty in configurations of nearby objects. 

4. Compute a path, from the manipulator’s initial configuration to the grasp approach 
configuration, assuming worst-case uncertainty. 

5. Identify a destination approach configuration, a configuration which is safe for the manipulator 
holding the object, given the uncertainty in the grasp configuration and the uncertainty of 
nearby objects. 

6 . Compute a safe path from the grasp deproach configuration to the destination approach 
configuration for die manipulator and the grasped part, also assuming wort-case uncertainty. 

7. Identify a destination depproach configuration, a configuration which is safe for the 
manipulator, given die uncertainty of nearby objects. 

This formulation of the synthesis problem factors out the problems of approaching and dcproach- 
ing both the nominal grasp configuration and the destination. For both of these problems, the use 
of sensory information to identify the actual state of the task and to accomodate to it is important 
[25] [30] [44]. When die uncertainty is small, the problem can be dealt with by ad hoc methods, 
c.g. opening the fingers very wide and relying on the grasping action to place the object and/or 
the manipulator in approximately die correct orientation [19]. The general problem of planning 
manipulator operations that are robust in the face of uncertainty is an important problem [30], but 
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beyond the scope of this paper. 


11. Summary 


This paper has presented an approach to the central geometric problems underlying die synthesis 
of "pick and place" motions for cartesian manipulators. The key technique in the approach is the use 
of explicit polyhedral representations of the configuration constraints on the manipulator. This repre¬ 
sentation permits the use of simple and powerful geometric operations to solve problems involving 
safe motions of the manipulator. In particular, the problems of finding grasp configurations and safe 


paths in the absence of uncertainty. 

The concepts of Configuration Space and Configuration Space Obstacle have played a central role 
in the approach to gross motion synthesis developed here. Similar concepts play an important role in 
the approach to compliant motion synthesis described in [30]. These concepts have also proven .useful 
in other geometric applications [1] |2] [4] [45]. 
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Figure 15. The WEDGE is a convex polyhedron used to approximate the volume swept out by a cuboid 
aligned with the coordinate axes, as it rotates around the z axis, assuming the z axis does not penetrate the 
cuboid 


Appendix 1. A Polyhedral Approximation for Swept Volume 


The swept volume is the volume occupied by a polyhedron over a set of configurations, e.g. along 
some path. The swept volume over a range of translations can be computed using the CO xyz algo¬ 
rithm. In this appendix, we will limit our attention to computing a simple polyhedral approximation 
to the swept volume for rotations of a polyhedron around an arbitrary axis. This method is included 
here for completeness, it is not the best polyhedral approximation to the swept volume. 

The swept volume approximation described here returns a list of convex polyhedra of two types: 

■ 

1. CYLINDER —a polyhedral approximation to a right circular cylinder. 
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x = Ay x- Ay 

2 2 


Figure 16. Computing a polyhedral approximation to the swept volume under pure rota¬ 
tion. 
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2. WEDGE — a polyhedral approximation to the volume swept out by a cuboid, aligned with 
the coordinate axes, as it rotates around the z axis, Figure 15. It assumes that the z axis does 
not penetrate the cuboid and that the rotation is less than ir. 

The input is a polyhedron, B, an axis of rotation which is the z axis of a reference frame and 0, 
the angle of rotation. The first step is to rotate the frame around z so that the x axis goes through 
the centroid of the projection of B on the ( x, y)-planc of the frame. Compute an aligned bounding 
rectangular solid for B , RB(B ), whose dimensions are (Ax, Ay, Az). If the z axis does not pass 
through the object, then if 0 < 0 inax < ir then simply return a WEDGE enclosing the swept volume. 
If the 2 axis penetrates RB(B), then if Ax > Ay, cut B using the planes x = ^ and x — zz p L , 
and return a cylinder of radius V2Ay whose height is Az and return the swept volumes of the pieces 
of B beyond the central area. The procedure is similar if Ay > Ax. Figure 16 illustrates this 
process. Here 0 max is some user specified parameter, although it could be chosen to guarantee some 
kind of error bound. If 0 > 0 max , then divide the rotation into a set of successive rotations each 
returning a wedge. 





